/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package roborealmtester;

/**
 *
 * @author Jarrod Risley
 */ 

import com.robos.api.RoboRealmAPI;
import java.util.Vector;
import static java.lang.System.out;

public class Main {

    /*
     * Global Class Variables
     */
    private static RoboRealmAPI rr;
    private static Vector blobVector;
    private static Vector coordinateVector;
    
    private int blobX;
    private int blobY;
    private int boundingXOne;
    private int boundingXTwo;
    private int rectangleLength;
    private double frameX;
    private double frameY;
    private final double targetHeight = 96; // This is in inches.
    private final double theta = 36; // This is in degrees.
    private final double dStar = 213.15;
    private final double t = 54;
    private double n = 4; // ONLY TRUE FOR 30deg... needs further testing.
    private double hypotenuse;    
    private final double heightOfThreePtGoal = 110.125;
    
    /**
     * @param args the command line arguments
     */
    public static void main(String[] args) {
        
        Main m = new Main();
        rr = new RoboRealmAPI();
        
        if (rr.connect("localhost")) {
            
            out.println("Connection Established with RoboRealm and"
                    + " KittyNet");
            
            while (true) {
               
                blobVector = rr.getVariables("BLOBS");
                coordinateVector = rr.getVariables("BOUNDING_COORDINATES");

                if (!blobVector.isEmpty()) {
                    
                    m.extractCenterCoordinates();
                    m.getCenterOfFrame();

                    rr.setVariable("DISTANCE",
                            Double.toString(m.getDistanceFromCenter()));

                    rr.setVariable("Distance_To_Target", 
                            Double.toString(m.getDistanceToTarget()));
                    
                    m.transmitDataToRobot();

                    out.println(m.getDistanceFromCenter());
                    out.println(m.getDistanceToTarget());
                    
                } else {
                    out.println("No targets in sight.");
                } // end inner if-else
            } // end while
        } else {
            out.println("Connection to RoboRealm and KittyNet Failed.");
        } // end if-else chain
        
    } // end main
    
    /**
     * Gets the coordinates of the first blob in the Vector, which is ordered 
     * from the blob highest in the frame to the lowest in the frame.
     */
    
    private void extractCenterCoordinates() {
        
        String extractedValues = (String) blobVector.get(0);
        
        String values[] = extractedValues.split(",");
        
        blobX = Integer.parseInt(values[0]);
        
        blobY = Integer.parseInt(values[1]);
        
        out.println("COG at: " + blobX + ", " + blobY);
        
    } // end extractCenterCoordinates
    
    /**
     * Grabs two of the rectangle's corners and calculates the length of the
     * corresponding side. The corresponding variable for this method is t'.
     */
    public int extractBoundingRectanglePoints() {
        
        String extractedCoordinates = (String) coordinateVector.get(0);
        
        String coordinates[] = extractedCoordinates.split(",");
        
        return Integer.parseInt(coordinates[0]) 
                - Integer.parseInt(coordinates[2]);
        
    } // end extractBoundingRectanglePoints
    
    /**
     * Grabs the center of the current frame.
     */
    
    private void getCenterOfFrame() {
        
        double frameWidth = rr.getDimension().getWidth();
        double frameHeight = rr.getDimension().getHeight();
        
        frameX = frameWidth / 2;
        frameY = frameHeight / 2;
        
    } // end getCenterOfFrame
    
    /**
     * Utilizes the good old Distance Formula to calculate the distance between
     * the center of the blob to the center of the frame.
     * 
     * PRECONDITION: Both the center of the blob and frame must be known.
     * 
     * @return How far the center of the blob is from the center of the frame,
     * expressed as a double.
     */
    
    private double getDistanceFromCenter() {
        
        return Math.sqrt(Math.pow(blobX - frameX, 2) + Math.pow(blobY - frameY, 2));
        
    } // end getDistanceFromCenter
    
    /**
     * Calculates the length of the hypotenuse of the right triangle between
     * the camera and the target using Evan's Equation, then uses trigonometry
     * to determine the horizontal distance to the target.
     * 
     * @return Horizontal Distance to target.
     * 
     * POSTCONDITION: hypotenuse is calculated.
     */
    
    private double getDistanceToTarget() {
        
        hypotenuse = (t / extractBoundingRectanglePoints()) * dStar;
        
        return Math.sqrt(Math.pow(hypotenuse, 2) - Math.pow(heightOfThreePtGoal, 2));
        
    } // end getDistanceToTarget
    
    public void transmitDataToRobot() {
        
        String horizontalDistance = Double.toString(getDistanceToTarget());
        String hypotuz = Double.toString(hypotenuse);
        String recenteringDistance = Double.toString(getDistanceFromCenter());
        
        rr.setVariable("horizontalDistance", horizontalDistance);
        rr.setVariable("hypotenuse", hypotuz);
        rr.setVariable("recenteringDistance", recenteringDistance);
        
    } // end transmitDataToRobot
    
} // end Main
